#include "MiniBSplineSurface.h"
#include "MiniBSplineCurve.h"
#include "MiniDiscreteCurve.h"

using namespace XYF;

#define MIN(a, b) ((a) > (b) ? (b) : (a))
#define MAX(a, b) ((a) < (b) ? (b) : (a))
#define SIGN(a) ((a) >= 0 ? 1 : -1)
#define ITERATION 10

MiniBSplineSurface::MiniBSplineSurface(const MiniArray2D<Vector3f> &cpts, const MiniArray<Real> &uKnots, const MiniArray<Real> &vKnots, const int uDegree, const int vDegree, const bool uPeriodic, const bool vPeriodic)
    : m_cpts(cpts), m_uKnots(uKnots), m_vKnots(vKnots), m_uDegree(uDegree), m_vDegree(vDegree),
      m_uPeriodic(uPeriodic), m_vPeriodic(vPeriodic)
{
}

MiniBSplineSurface::MiniBSplineSurface(const int Nu, const int Nv, const int uDegree, const int vDegree)
    : m_cpts(Nu, MiniArray<Vector3f>(Nv)), m_uDegree(uDegree), m_vDegree(vDegree), m_uPeriodic(false), m_vPeriodic(false)
{
    // 不处理周期情况
    int p = uDegree;
    int q = vDegree;

    // u 节点
    for (int i = 0; i < p + 1; i++)
        m_uKnots.push_back(0);
    for (int i = p + 1; i < Nu; i++)
        m_uKnots.push_back(1.0 * (i - p) / (Nu - p));
    for (int i = Nu; i < Nu + p + 1; i++)
        m_uKnots.push_back(1);

    // v 节点
    for (int i = 0; i < q + 1; i++)
        m_vKnots.push_back(0);
    for (int i = q + 1; i < Nv; i++)
        m_vKnots.push_back(1.0 * (i - q) / (Nv - q));
    for (int i = Nv; i < Nv + q + 1; i++)
        m_vKnots.push_back(1);
}

int MiniBSplineSurface::GetType()
{
    return MiniObject::B_spline_surface;
}

const MiniArray2D<Vector3f> &MiniBSplineSurface::GetPoints() const
{
    return m_cpts;
}

MiniArray2D<Vector3f> &MiniBSplineSurface::GetPoints()
{
    return m_cpts;
}

int MiniBSplineSurface::GetUPointsNum() const
{
    return m_cpts.size();
}

int MiniBSplineSurface::GetVPointsNum() const
{
    return m_cpts.front().size();
}

int MiniBSplineSurface::GetUDegree() const
{
    return m_uDegree;
}

int MiniBSplineSurface::GetVDegree() const
{
    return m_vDegree;
}

bool MiniBSplineSurface::GetUPeriodic() const
{
    return m_uPeriodic;
}

bool MiniBSplineSurface::GetVPeriodic() const
{
    return m_vPeriodic;
}

MiniArray<Real> MiniBSplineSurface::GetUKnots() const
{
    return m_uKnots;
}

MiniArray<Real> MiniBSplineSurface::GetVKnots() const
{
    return m_vKnots;
}

MiniArray<Real> &MiniBSplineSurface::GetUKnots()
{
    return m_uKnots;
}

MiniArray<Real> &MiniBSplineSurface::GetVKnots()
{
    return m_vKnots;
}

Real MiniBSplineSurface::GetUBegin() const
{
    return m_uKnots[m_uDegree];
}

Real MiniBSplineSurface::GetVBegin() const
{
    return m_vKnots[m_vDegree];
}

Real MiniBSplineSurface::GetUEnd() const
{
    return m_uKnots[m_uKnots.size() - m_uDegree - 1];
}

Real MiniBSplineSurface::GetVEnd() const
{
    return m_vKnots[m_vKnots.size() - m_vDegree - 1];
}

Vector3f MiniBSplineSurface::SurfacePoint(Real u, Real v) const
{
    int p = GetUDegree();
    int q = GetVDegree();
    int uSpan = FindSpan(u, p, m_uKnots);
    int vSpan = FindSpan(v, q, m_vKnots);
    MiniArray<Real> Nu = BasisFuncs(uSpan, u, p, m_uKnots);
    MiniArray<Real> Nv = BasisFuncs(vSpan, v, q, m_vKnots);

    Vector3f S;
    for (int i = 0; i < q + 1; i++)
    {
        Vector3f tmp;
        for (int j = 0; j < p + 1; j++)
            tmp = tmp + m_cpts[uSpan - p + j][vSpan - q + i] * Nu[j];
        S = S + tmp * Nv[i];
    }
    return S;
}

MiniArray4D<Vector3f> MiniBSplineSurface::SurfaceDerivCpts(int d, int r1, int r2, int s1, int s2) const
{
    int p = GetUDegree();
    int q = GetVDegree();
    int du = MIN(d, p);
    int dv = MIN(d, q);
    int r = r2 - r1;
    int s = s2 - s1;

    // 保存 P_{i,j}^(k,l) 对应 PKL[k][l][i][j]
    MiniArray4D<Vector3f> PKL(du + 1, MiniArray3D<Vector3f>(dv + 1, MiniArray2D<Vector3f>(r + 1, MiniArray<Vector3f>(s + 1))));

    // 先处理 u 方向，得到 P_{i,j}^(k,0) 即 PKL[k][0][i][j]
    // 对 j 即 v 方向分别计算 u 方向的 k 阶导控制点
    for (int j = 0; j < s + 1; j++)
    {
        MiniArray2D<Vector3f> PK(du + 1, MiniArray<Vector3f>(r + 1));

        // 0 阶导控制点（初始控制点） P_i^0
        for (int i = 0; i < r + 1; i++)
            PK[0][i] = m_cpts[r1 + i][s1 + j];

        // 计算 P_i^k
        for (int k = 1; k < du + 1; k++)
        {
            int tmp = p - k + 1;
            // P_i^k = (P_{i+1}^{k-1}-P_i^{k-1}) / (u_{i+p+1}-u_{i+k}) * (p-k+1)
            for (int i = 0; i < r - k + 1; i++)
                PK[k][i] = (PK[k - 1][i + 1] - PK[k - 1][i]) / (m_uKnots[r1 + i + p + 1] - m_uKnots[r1 + i + k]) * tmp;
        }

        // 储存计算结果，这部分只和 k,i 有关
        for (int k = 0; k < du + 1; k++)
            for (int i = 0; i < r - k + 1; i++)
                PKL[k][0][i][j] = PK[k][i];
    }

    // 再处理 v 方向，对于不同的 P_{i,j}^(k,0) 中的 k 计算 v 方向导
    for (int k = 0; k < du; k++)
    {
        // 对不同的 k，在 u 方向上计算出的控制点数为 r-k+1，因此对 i 从 0 到 r-k 操作
        for (int i = 0; i < r - k + 1; i++)
        {
            // u,v 方向导 不超过 d，这里减去已经对 u 求的 k 次导
            int dd = MIN(d - k, dv);
            MiniArray2D<Vector3f> PK(dd + 1, MiniArray<Vector3f>(s + 1));

            // 0 阶导控制点（初始控制点），即从 PKL 中获得 P_{i,j}^(k,0) 控制点
            for (int j = 0; j < s + 1; j++)
                PK[0][j] = PKL[k][0][i][j];

            // 计算 P_{i,j}^(k,l)
            for (int l = 1; l < dd + 1; l++)
            {
                int tmp = q - l + 1;
                for (int j = 0; j < s - l + 1; j++)
                    PK[l][j] = (PK[l - 1][j + 1] - PK[l - 1][j]) / (m_vKnots[s1 + j + q + 1] - m_vKnots[s1 + j + l]) * tmp;
            }

            // 最后获得控制点 P_{i,j}^(k,l)，这里只和 l,j 有关
            for (int l = 0; l < dd + 1; l++)
                for (int j = 0; j < s - l + 1; j++)
                    PKL[k][l][i][j] = PK[l][j];
        }
    }
    return PKL;
}

MiniArray2D<Vector3f> MiniBSplineSurface::SurfaceDerivPoint(int d, Real u, Real v) const
{
    MiniArray2D<Vector3f> SKL(d + 1, MiniArray<Vector3f>(d + 1));

    int p = GetUDegree();
    int q = GetVDegree();
    int uSpan = FindSpan(u, p, m_uKnots);
    int vSpan = FindSpan(v, q, m_vKnots);
    int du = MIN(d, p);
    int dv = MIN(d, q);

    MiniArray2D<Real> Nu = AllBasisFuncs(uSpan, u, p, m_uKnots);
    MiniArray2D<Real> Nv = AllBasisFuncs(vSpan, v, q, m_vKnots);
    MiniArray4D<Vector3f> PKL = SurfaceDerivCpts(d, uSpan - p, uSpan, vSpan - q, vSpan);

    for (int k = 0; k < du + 1; k++)
    {
        int dd = MIN(d - k, dv);
        for (int l = 0; l < dd + 1; l++)
        {
            for (int i = 0; i < q - l + 1; i++)
            {
                // N[j][r] = N_{r,j}
                Vector3f tmp;
                for (int j = 0; j < p - k + 1; j++)
                    tmp = tmp + PKL[k][l][j][i] * Nu[p - k][j];
                SKL[k][l] = SKL[k][l] + tmp * Nv[q - l][i];
            }
        }
    }
    return SKL;
}

void MiniBSplineSurface::SurfaceKnotIns(Real t, int r, char para)
{
    int p = (para == 'u') ? GetUDegree() : GetVDegree();
    int n = (para == 'u') ? GetUPointsNum() - 1 : GetVPointsNum() - 1;
    int l = (para == 'v') ? GetUPointsNum() - 1 : GetVPointsNum() - 1;
    MiniArray<Real> &knots = (para == 'u') ? m_uKnots : m_vKnots;

    // 扩大控制点数组
    if (para == 'u')
    {
        // 将 u 方向的数组全部都重置大小
        m_cpts.resize(n + r + 1);
        for (int i = 0; i < r; i++)
            m_cpts[n + i + 1].resize(l + 1);
    }
    else
    {
        for (int i = 0; i < l + 1; i++)
            m_cpts[i].resize(n + r + 1);
    }

    // 在另一个方向上有 l + 1 个点，共有 l + 1 条曲线
    MiniArray<Real> U;
    for (int i = 0; i < l + 1; i++)
    {
        // 保存这个方向上的控制点
        MiniArray<Vector3f> cpts(n + 1);
        for (int j = 0; j < n + 1; j++)
        {
            if (para == 'u')
                cpts[j] = m_cpts[j][i];
            else
                cpts[j] = m_cpts[i][j];
        }

        // 计算扩大的控制点列
        MiniBSplineCurve spline(cpts, knots, p);
        spline.CurveKnotIns(t, r);
        const MiniArray<Vector3f> &P = spline.GetPoints();

        // 记录插入后的新控制点
        for (int j = 0; j < n + r + 1; j++)
        {
            if (para == 'u')
                m_cpts[j][i] = P[j];
            else
                m_cpts[i][j] = P[j];
        }

        // 只记录一次节点向量
        if (i == 0)
            U = spline.GetKnots();
    }

    // 最后修改节点
    knots = U;
}

void MiniBSplineSurface::SurfaceRefineKnot(const MiniArray<Real> &X, char para)
{
    int p = (para == 'u') ? GetUDegree() : GetVDegree();
    int n = (para == 'u') ? GetUPointsNum() - 1 : GetVPointsNum() - 1;
    int l = (para == 'v') ? GetUPointsNum() - 1 : GetVPointsNum() - 1;
    int r = X.size();
    MiniArray<Real> &knots = (para == 'u') ? m_uKnots : m_vKnots;

    // 扩大控制点数组
    if (para == 'u')
    {
        // 将 u 方向的数组全部都重置大小
        m_cpts.resize(n + r + 1);
        for (int i = 0; i < r; i++)
            m_cpts[n + i + 1].resize(l + 1);
    }
    else
    {
        for (int i = 0; i < l + 1; i++)
            m_cpts[i].resize(n + r + 1);
    }

    // 在另一个方向上有 l + 1 个点，共有 l + 1 条曲线
    MiniArray<Real> U;
    for (int i = 0; i < l + 1; i++)
    {
        // 保存这个方向上的控制点
        MiniArray<Vector3f> cpts(n + 1);
        for (int j = 0; j < n + 1; j++)
        {
            if (para == 'u')
                cpts[j] = m_cpts[j][i];
            else
                cpts[j] = m_cpts[i][j];
        }

        // 计算扩大的控制点列
        MiniBSplineCurve spline(cpts, knots, p);
        spline.CurveRefineKnot(X);
        const MiniArray<Vector3f> &P = spline.GetPoints();

        // 记录插入后的新控制点
        for (int j = 0; j < n + r + 1; j++)
        {
            if (para == 'u')
                m_cpts[j][i] = P[j];
            else
                m_cpts[i][j] = P[j];
        }

        // 只记录一次节点向量
        if (i == 0)
            U = spline.GetKnots();
    }

    // 最后修改节点
    knots = U;
}

Vector MiniBSplineSurface::NearPointProjection(Vector3f P, Vector uv, Real tol)
{
    // 应用牛顿迭代法
    int times = 0;
    LinearSolver solver;

    Real dp = tol * 10;
    Real near = distance(P, SurfacePoint(uv[0], uv[1]));
    while (near > tol && dp > tol && times < ITERATION)
    {
        MiniArray2D<Vector3f> S = SurfaceDerivPoint(2, uv[0], uv[1]);
        Vector3f dS = S[0][0] - P;
        Real lSu = length(S[1][0]);
        Real lSv = length(S[0][1]);

        Matrix J(2, 2);
        J(0, 0) = pow(lSu, 2) + dot(S[2][0], dS);
        J(0, 1) = J(1, 0) = dot(S[1][0], S[0][1]) + dot(S[1][1], dS);
        J(1, 1) = pow(lSv, 2) + dot(S[0][2], dS);

        Vector kappa(2);
        kappa[0] = -dot(S[1][0], dS);
        kappa[1] = -dot(S[0][1], dS);

        Vector duv = solver.solve(J, kappa, true);
        uv = uv + duv;

        // 防止越界
        if (uv[0] <= GetUBegin())
            uv[0] = GetUBegin() + tol;
        if (uv[0] >= GetUEnd())
            uv[0] = GetUEnd() - tol;
        if (uv[1] <= GetVBegin())
            uv[1] = GetVBegin() + tol;
        if (uv[1] >= GetVEnd())
            uv[1] = GetVEnd() - tol;

        near = distance(P, SurfacePoint(uv[0], uv[1]));
        dp = fabs(lSu * duv[0] + lSv * duv[1]);

        times++;
    }
    return uv;
}

Vector MiniBSplineSurface::OrthoPointProjection(Vector3f P, Vector uv, Real tol)
{
    // 曲率投影
    int times = 0;
    Real angle = tol * 10;

    LinearSolver solver;
    while (angle > tol && times < ITERATION)
    {
        MiniArray2D<Vector3f> S = SurfaceDerivPoint(2, uv[0], uv[1]);
        Vector3f dP = P - S[0][0];

        // 获得 I,II 基本形式的矩阵
        Vector3f N = normalize(cross(S[1][0], S[0][1]));
        Matrix I(2, 2, {dot(S[1][0], S[1][0]), dot(S[1][0], S[0][1]), dot(S[0][1], S[1][0]), dot(S[0][1], S[0][1])});
        Matrix II(2, 2, {dot(S[2][0], N), dot(S[1][1], N), dot(S[1][1], N), dot(S[0][2], N)});

        // 沿着 Su Sv 方向进行分解（利用线性方程组）
        Vector b = {dot(dP, S[1][0]), dot(dP, S[0][1])};
        Vector lambda = solver.solve(I, b);

        // 计算法曲率
        Real h = 0;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                h += II(i, j) * lambda[i] * lambda[j];

        Real g = 0;
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                g += I(i, j) * lambda[i] * lambda[j];

        // 法曲率，法曲率圆心，投影点 Q
        Real kappa = h / g;
        Real R = fabs(1 / kappa);
        Vector3f O = S[0][0] + N / kappa;
        Vector3f Q = O + normalize(P - O) * R;

        // 计算增量
        Real dt = SIGN(dot(S[1][0] * lambda[0] + S[0][1] * lambda[1], Q - S[0][0]));
        uv[0] += dt * lambda[0];
        uv[1] += dt * lambda[1];

        // 防止越界
        if (uv[0] <= GetUBegin())
            uv[0] = GetUBegin() + tol;
        if (uv[0] >= GetUEnd())
            uv[0] = GetUEnd() - tol;
        if (uv[1] <= GetVBegin())
            uv[1] = GetVBegin() + tol;
        if (uv[1] >= GetVEnd())
            uv[1] = GetVEnd() - tol;

        // 夹角足够小
        angle = length(cross(N, P - S[0][0])) / length(P - S[0][0]);
        times++;
    }
    return uv;
}

#include <QDebug>

Vector MiniBSplineSurface::OrthoPointTorusProjection(Vector3f P, Vector uv, Real tol)
{
    // 曲率投影
    int times = 0;
    Real angle = tol * 10;

    LinearSolver solver;
    FSolve fS;
    while (angle > tol && times < ITERATION)
    {
        MiniArray2D<Vector3f> S = SurfaceDerivPoint(2, uv[0], uv[1]);

        // 获得 I,II 基本形式的矩阵
        Vector3f N = normalize(cross(S[1][0], S[0][1]));
        Matrix I(2, 2, {dot(S[1][0], S[1][0]), dot(S[1][0], S[0][1]), dot(S[0][1], S[1][0]), dot(S[0][1], S[0][1])});
        Matrix II(2, 2, {dot(S[2][0], N), dot(S[1][1], N), dot(S[1][1], N), dot(S[0][2], N)});

        // 算出主曲率和主方向
        Real H = (II(0, 0) * I(1, 1) - 2 * II(1, 0) * I(1, 0) + II(1, 1) * I(0, 0)) / I.det() / 2;
        Real K = II.det() / I.det();

        // 如果当前位置是脐点，就转而使用法曲率圆投影
        Real Delta = H * H - K;
        if (Delta <= 0)
            return OrthoPointProjection(P, uv, tol);

        // 主曲率
        Real k1 = H + sqrt(Delta);
        Real k2 = H - sqrt(Delta);

        // 主方向
        Vector3f e1 = S[1][0] * (II(1, 0) - k1 * I(1, 0)) / (II(0, 0) - k1 * I(0, 0)) - S[0][1];
        Vector3f e2 = S[1][0] * (II(1, 0) - k2 * I(1, 0)) / (II(0, 0) - k2 * I(0, 0)) - S[0][1];
        e1 = normalize(e1);
        e2 = normalize(e2);

        // 确保 |k1| <= |k2|
        if (fabs(k1) > fabs(k2))
        {
            Real tk = k1;
            k1 = k2;
            k2 = tk;

            Vector3f ek = e1;
            e1 = e2;
            e2 = ek;
        }
        // 确保 k1 <= 0
        if (k1 > 0)
        {
            N = N * -1;
            k1 *= -1;
            k2 *= -1;
        }

        // 建立环面，得到投影点 Pr
        Real R = -1 / k1 + 1 / k2;
        Real r = fabs(1 / k2);
        Vector3f O = S[0][0] + N / k1;
        Vector3f OP = P - O;
        Vector3f OB = normalize(OP - e2 * dot(OP, e2)) * R;
        Vector3f Pr = O + OB + normalize(OP - OB) * r;

        // 计算初始增量
        Vector3f QPr = Pr - S[0][0];

        // 沿着 Su Sv 方向进行分解（利用线性方程组），计算参数初始增量 duv
        Vector b = {dot(QPr, S[1][0]), dot(QPr, S[0][1])};
        Vector duv = solver.solve(I, b);

        // 使用牛顿迭代法计算根
        FuncXX f = [=](Vector UV)
        {
            Vector3f E = S[1][0] * UV[0] + S[0][1] * UV[1] +
                    (S[2][0] * UV[0] * UV[0] +
                    S[1][1] * UV[0] * UV[1] * 2 +
                    S[0][2] * UV[1] * UV[1]) / 2 - QPr;
            return Vector({E.x(), E.y(), E.z()});
        };

        // 获得增量
        fS(f, duv);
        uv = uv + duv;

        // 防止越界
        if (uv[0] <= GetUBegin())
            uv[0] = GetUBegin() + tol;
        if (uv[0] >= GetUEnd())
            uv[0] = GetUEnd() - tol;
        if (uv[1] <= GetVBegin())
            uv[1] = GetVBegin() + tol;
        if (uv[1] >= GetVEnd())
            uv[1] = GetVEnd() - tol;

        // 夹角足够小
        angle = length(cross(N, P - Pr)) / length(P - Pr);
        times++;
    }
    return uv;
}
